#!/usr/bin/env python
import rospy
#import roslib
#roslib.load_manifest('differential_drive')
import math
from math import sin, cos, pi
from geometry_msgs.msg import Quaternion
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from tf.broadcaster import TransformBroadcaster
from std_msgs.msg import Int16, Int64, Float32

#############################################################################
class DiffTf:
#############################################################################
    #############################################################################
    def __init__(self):
    #############################################################################
        rospy.init_node("omni_tf")
        self.nodename = rospy.get_name()
        rospy.loginfo("-I- %s started" % self.nodename)
        
        #### parameters #######
        self.rate = rospy.get_param('~rate',60.0)  # the rate at which to publish the transform
        self.ticks_meter = float(rospy.get_param('ticks_meter', 1600))  # The number of wheel encoder ticks per meter of travel
	print "ticks meter ", self.ticks_meter
        self.base_width = float(rospy.get_param('~base_width', 0.31)) # The wheel base width in meters
        self.base_length = float(rospy.get_param('~base_length', 0.34)) # The wheel base length in meters  *******changed*******
        self.r = rospy.get_param("~wheel_radius", 0.076) #**********
        self.base_frame_id = rospy.get_param('~base_frame_id','base_link') # the name of the base frame of the robot  base_link
        self.odom_frame_id = rospy.get_param('~odom_frame_id', 'odom') # the name of the odometry reference frame
        self.encoder_min = rospy.get_param('encoder_min', -2147483648)
        self.encoder_max = rospy.get_param('encoder_max', 2147483648)
        self.x_scale = float(rospy.get_param('x_scale', 1.0))
        self.y_scale = float(rospy.get_param('y_scale', 1.0))  
        self.th_scale = float(rospy.get_param('th_scale', 1.0))  
        self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
        self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
 
        self.t_delta = rospy.Duration(1.0/self.rate)
        self.t_next = rospy.Time.now() + self.t_delta
        
        # internal data
        self.enc_left_f = 0        # delta encoder readings
        self.enc_right_f = 0
        self.enc_left_r = 0       
        self.enc_right_r = 0

        self.prev_lfencoder = 0
        self.prev_rfencoder = 0  
        self.prev_lrencoder = 0        
        self.prev_rrencoder = 0
        self.prev_angular = 0

        self.enc_left_f_init = 0
        self.enc_right_f_init = 0
        self.enc_left_r_init = 0       
        self.enc_right_r_init = 0
        self.enc_left_f_has_init = False
        self.enc_right_f_has_init = False
        self.enc_left_r_has_init = False       
        self.enc_right_r_has_init = False

	self.has_angular = False

        self.x = 0.0                  # position in xy plane 
        self.y = 0.0
        self.th = 0
        self.dx = 0                 # speeds in x/rotation
        self.dy = 0 
        self.dr = 0
        self.vx = 0
        self.vth = 0
        self.vy = 0
        self.angular = 0
        self.then = rospy.Time.now()
        
        # subscriptions
        rospy.Subscriber("lfwheel", Int64, self.lfwheelCallback)  
        rospy.Subscriber("rfwheel", Int64, self.rfwheelCallback)
        rospy.Subscriber("lrwheel", Int64, self.lrwheelCallback)
        rospy.Subscriber("rrwheel", Int64, self.rrwheelCallback)
        rospy.Subscriber('cmd_vel', Twist, self.velocityCallback)
        rospy.Subscriber('angular', Float32, self.angularCallback)
        self.odomPub = rospy.Publisher("odom", Odometry,queue_size=10)
        self.odomBroadcaster = TransformBroadcaster()
        print "Pos ", self.x, self.y, self.th
    #############################################################################
    def spin(self):
    #############################################################################
        r = rospy.Rate(self.rate)
        while not rospy.is_shutdown():
            self.update()
            r.sleep() 
    #############################################################################
    def update(self):
    #############################################################################
	if not self.has_angular:
	    return
        now = rospy.Time.now()
        if now > self.t_next:
            elapsed = now - self.then
            self.then = now
            elapsed = elapsed.to_sec()

            encoder1 = self.enc_left_f 
            encoder2 = self.enc_right_f 
            encoder3 = self.enc_left_r
            encoder4 = self.enc_right_r
	    self.th = self.angular*pi/180 
            
            delta1= encoder1 - self.prev_lfencoder
            delta2= encoder2 - self.prev_rfencoder
            delta3= encoder3 - self.prev_lrencoder
            delta4= encoder4 - self.prev_rrencoder
            
            self.prev_lfencoder = encoder1
            self.prev_rfencoder = encoder2
            self.prev_lrencoder = encoder3
            self.prev_rrencoder = encoder4

            xcounts = (delta1+delta2+delta3+delta4)/4
            ycounts = (-delta1+delta2+delta3-delta4)/4
            thcounts = (delta1-delta2-delta3+delta4)/4/(self.base_width+self.base_length)
            dx = (float)(xcounts /self.ticks_meter/self.x_scale)
            dy = (float)(ycounts /self.ticks_meter/self.y_scale)
            delta5 = self.th - self.prev_angular
            #delta5 = (float)(thcounts /self.ticks_meter/self.th_scale)
	    #self.prev_angular = delta5
            self.x += cos(self.th)*dx - sin(self.th )*dy
            self.y += sin(self.th)*dx + cos(self.th )*dy
	    if False and dx != 0 and dy !=0:
		print self.enc_left_f, self.prev_lfencoder
		print "delta ", delta1, delta2, delta3, delta4
		print "counts ", xcounts, ycounts
  	        print self.x, dx, cos(self.th), sin(self.th), dy, self.th, self.angular
		print "change ", cos(self.th)*dx - sin(self.th )*dy
            self.prev_angular = self.th      
	    #print "Pos ", self.x, self.y, self.th
            #self.th += delta5
            self.dx =(float) (dx) /elapsed
            self.dy = (float)(dy) /elapsed            
            self.dr = (float)(delta5) /elapsed
                
            # publish the odom information
            quaternion = Quaternion()
            quaternion.x = 0.0
            quaternion.y = 0.0
            quaternion.z = sin( self.th /2)
            quaternion.w = cos( self.th /2)
            self.odomBroadcaster.sendTransform(
                (self.x, self.y, 0),
                (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
                rospy.Time.now(),
                self.base_frame_id,
                self.odom_frame_id
                )           
            odom = Odometry()
            odom.header.stamp = now
            odom.header.frame_id = self.odom_frame_id
            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.position.z = 0
            odom.pose.pose.orientation = quaternion
            odom.child_frame_id = self.base_frame_id
            odom.twist.twist.linear.x = self.dx
            odom.twist.twist.linear.y = self.dy
            odom.twist.twist.angular.z = self.dr
            self.odomPub.publish(odom)

    #############################################################################
    def lfwheelCallback(self, msg):
    #############################################################################
        enc = msg.data
	if not self.enc_left_f_has_init:
	    self.enc_left_f_has_init = True
	    self.enc_left_f_init = enc
	    self.enc_left_f = 0 	
	else:
	    self.enc_left_f = enc - self.enc_left_f_init
    #############################################################################
    def rfwheelCallback(self, msg):
    #############################################################################
        enc = msg.data
	if not self.enc_right_f_has_init:
	    self.enc_right_f_has_init = True
	    self.enc_right_f_init = enc
	    self.enc_right_f = 0 	
	else:
	    self.enc_right_f = enc - self.enc_right_f_init
#############################################################################
    def lrwheelCallback(self, msg):
    #############################################################################
        enc = msg.data
	if not self.enc_left_r_has_init:
	    self.enc_left_r_has_init = True
	    self.enc_left_r_init = enc
	    self.enc_left_r = 0 	
	else:
	    self.enc_left_r = enc - self.enc_left_r_init
#############################################################################
    def rrwheelCallback(self, msg):
    #############################################################################
        enc = msg.data
	if not self.enc_right_r_has_init:
	    self.enc_right_r_has_init = True
	    self.enc_right_r_init = enc
	    self.enc_right_r = 0 	
	else:
	    self.enc_right_r = enc - self.enc_right_r_init
#############################################################################
    def velocityCallback(self,msg):
        # rospy.loginfo("-D- twistCallback: %s" % str(msg))
        self.vx = msg.linear.x
        self.vth = msg.angular.z
        self.vy = msg.linear.y
#############################################################
    def angularCallback(self,msg):
        self.angular = msg.data
	if not self.has_angular:
	   self.has_angular = True;
###########################################################################

if __name__ == '__main__':
    """ main """
    diffTf = DiffTf()
diffTf.spin()
